#include <malloc.h>
#include <arch/mem.h>
#include <core/types.h>
#include <core/init.h>
#include <core/list.h>
#include <core/errno.h>
#include <arch/arm.h> // fixme


#define LIST_NODE_SIZE             WORD_ALIGN_UP(sizeof(struct ListNode))
#define LIST_NODE_ALIGN(nSize)     (((nSize) + LIST_NODE_SIZE - 1) & ~(LIST_NODE_SIZE - 1))
#define MIN_HEAP_LEN               1024
#define IS_FREE(nSize)             (((nSize) & (WORD_SIZE - 1)) == 0)
#define GET_SIZE(pRegion)          ((pRegion)->nCurrSize & ~(WORD_SIZE - 1))


struct MemRegion
{
	CPUWORD   nPrevSize;
	CPUWORD   nCurrSize;
	struct ListNode lnMemRegion;
};


static struct ListNode g_listFreeRegion;


static __INLINE__ struct MemRegion *GetSuccessor(struct MemRegion *pRegion)
{
	return (struct MemRegion *)((BYTE *)pRegion + DWORD_SIZE + GET_SIZE(pRegion));
}


static __INLINE__ struct MemRegion *GetPredeccessor(struct MemRegion *pRegion)
{
	return (struct MemRegion *)((BYTE *)pRegion - (pRegion->nPrevSize & ~(WORD_SIZE - 1)) - DWORD_SIZE);
}


static __INLINE__ void RegionSetSize(struct MemRegion *pRegion, CPUWORD nSize)
{
	struct MemRegion *pSuccRegion;

	pRegion->nCurrSize = nSize;

	pSuccRegion = GetSuccessor(pRegion);
	pSuccRegion->nPrevSize = nSize;
}


int GkInitHeap(unsigned long start, unsigned long end)
{
	struct MemRegion *pFirst, *pTail;

	start = WORD_ALIGN_UP(start);
	end   = WORD_ALIGN_DOWN(end);

	if (start + MIN_HEAP_LEN >= end)
		return -EINVAL;

	pFirst = (struct MemRegion *)start;
	pTail  = (struct MemRegion *)(end - DWORD_SIZE);

	pFirst->nPrevSize = 1;
	pFirst->nCurrSize = (CPUWORD)pTail - (CPUWORD)pFirst - DWORD_SIZE;

	pTail->nPrevSize = pFirst->nCurrSize;
	pTail->nCurrSize = 1;

	InitListHead(&g_listFreeRegion);

	ListAddTail(&pFirst->lnMemRegion, &g_listFreeRegion);

	return 0;
}


void *malloc(ULONG nSize)
{
	void *p = NULL;
	struct ListNode *iter;
	ULONG nAllocSize, nRestSize;
	struct MemRegion *pCurrRegion, *pPrevRegion, *pSuccRegion;
	UINT32 psr;

	LockIrqPsr(psr);

	nAllocSize = LIST_NODE_ALIGN(nSize);
	LIST_FOR_EACH(iter, &g_listFreeRegion)
	{
		pCurrRegion = MemToObj(iter, struct MemRegion, lnMemRegion);
		// printf("%d <--> %d\n", pCurrRegion->nCurrSize, nAllocSize);
		if (pCurrRegion->nCurrSize >= nAllocSize)
			goto do_alloc;
	}

	UnlockIrqPsr(psr);

	return NULL;

do_alloc:
	ListDelNode(iter);

	nRestSize = pCurrRegion->nCurrSize - nAllocSize;

	if (nRestSize < sizeof(struct MemRegion))
	{
		RegionSetSize(pCurrRegion, pCurrRegion->nCurrSize | 1);
	}
	else
	{
		RegionSetSize(pCurrRegion, nAllocSize | 1);

		pSuccRegion = GetSuccessor(pCurrRegion);
		RegionSetSize(pSuccRegion, nRestSize - DWORD_SIZE);
		ListAddTail(&pSuccRegion->lnMemRegion, &g_listFreeRegion);
	}

	p = &pCurrRegion->lnMemRegion;

	UnlockIrqPsr(psr);

	return p;
}


void free(void *p)
{
	struct MemRegion *pCurrRegion, *pSuccRegion;
	UINT32 psr;

	LockIrqPsr(psr);

	pCurrRegion = (struct MemRegion *)((CPUWORD)p - DWORD_SIZE);
	pSuccRegion = GetSuccessor(pCurrRegion);

	if (IS_FREE(pSuccRegion->nCurrSize))
	{
		RegionSetSize(pCurrRegion, GET_SIZE(pCurrRegion) + pSuccRegion->nCurrSize + DWORD_SIZE);
		ListDelNode(&pSuccRegion->lnMemRegion);
	}
	else
	{
		RegionSetSize(pCurrRegion, GET_SIZE(pCurrRegion));
	}

	if (IS_FREE(pCurrRegion->nPrevSize))
	{
		struct MemRegion *pPrevRegion;

		pPrevRegion = GetPredeccessor(pCurrRegion);
		RegionSetSize(pPrevRegion, pPrevRegion->nCurrSize + pCurrRegion->nCurrSize + DWORD_SIZE);
	}
	else
	{
		ListAddTail(&pCurrRegion->lnMemRegion, &g_listFreeRegion);
	}

	UnlockIrqPsr(psr);
}



void *kzalloc(ULONG len, int flag)
{
	void *p;

	p = kmalloc(len, flag);

	if (p)
		memset(p, 0, len);

	return p;
}

void *DmaMemAlloc(size_t len, ULONG *pPhyAddr)
{
	void *p;

	p = malloc(len);

	*pPhyAddr = (ULONG)p;

	return p;
}

